import serial
import time
import sys

class usbtty():
	def get_crc(self, dat:bytes) -> bytes:
		crc = 0xffff
		for c in dat:
			crc = crc ^ c
			for i in range(8):
				if (crc & 1) > 0:
					crc = crc >> 1
					crc = crc ^ 0xa001
				else:
					crc = crc >> 1
		return crc.to_bytes(2, "little", signed=False)

	def __init__(self, ttypath:str):
		self.serial	= serial.Serial(ttypath, 115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE)
		self.serial._timeout = 1
		pass
	def write_frame(self, addr:int, reg:int, dat:bytes) -> bytes:
		frame = addr.to_bytes(1, "big", signed=False) + b"\x06" + reg.to_bytes(2, "big", signed=False) + dat
		crc = self.get_crc(frame)
		self.serial.write(frame + crc)
		return self.serial.read(10)
	def read_frame(self, addr:int, reg:int, cnt:int):
		frame = addr.to_bytes(1, "big", signed=False) + b"\x03" + reg.to_bytes(2, "big", signed=False) + cnt.to_bytes(2, "big", signed=False)
		crc = self.get_crc(frame)
		self.serial.write(frame + crc)
		recv = self.serial.read(5 + cnt * 2)
		return recv
		

class laser(usbtty):
	def __init__(self, ttypath:str, addr:int):
		super().__init__(ttypath)
		self.addr = addr
	def write_frame(self, addr: int, reg: int, dat: bytes) -> bytes:
		recv = super().write_frame(addr, reg, dat)
		if recv:
			print("    << %s", recv)
			if self.get_crc(recv[0:-2]) == recv[-2:]:
				print("    done")
			else:
				print("    crc check failed")
		else:
			print("    no response")
		return recv
	def read_frame(self, addr: int, reg: int, cnt: int):
		recv = super().read_frame(addr, reg, cnt)
		if recv:
			print("    << %s", recv)
			if self.get_crc(recv[0:-2]) == recv[-2:] and cnt*2 == recv[2]:
				return recv[3:3+cnt*2]
			else:
				print("    crc check or length check failed")
				return None
		else:
			print("    no response")
			return None

	def save_rom(self):
		print("write parameters to rom...")
		self.write_frame(self.addr, 0x18, b"\x00\x01")
	
	def config_can_baud(self, baud:int):
		print("set can baud rate to %d kbps..." % (baud))
		self.write_frame(self.addr, 0x15, baud.to_bytes(2, "big", signed=False))
	
	def config_can_send_id(self, id:int):
		print("set can send id to %d (0x%x)..." % (id, id))
		self.write_frame(self.addr, 0x16, id.to_bytes(4, "big", signed=False))
	
	def config_can_recv_id(self, id:int):
		print("set can recv id to %d (0x%x)..." % (id, id))
		self.write_frame(self.addr, 0x17, id.to_bytes(4, "big", signed=False))

	def config_sample_freq(self, level:int):
		print("set sample frequency level to %d..." % (level))
		self.write_frame(self.addr, 0x07, level.to_bytes(2, "big", signed=False))

	def check_can_baud(self):
		print("checking can baud rate...")
		recv = self.read_frame(self.addr, 0x15, 1)
		if recv:
			baud = int.from_bytes(recv, "big", signed=False)
			print("got can baud rate: %d kbps" % (baud))
			return baud
		else:
			print("failed to read")
			return None
	
	def check_can_send_id(self):
		print("checking can send id...")
		recv = self.read_frame(self.addr, 0x16, 2)
		if recv:
			id = int.from_bytes(recv, "big", signed=False)
			print("got can send id: %d (0x%x)" % (id, id))
			return id
		else:
			print("failed to read")
			return None
	
	def check_can_recv_id(self):
		print("checking can recv id...")
		recv = self.read_frame(self.addr, 0x17, 2)
		if recv:
			id = int.from_bytes(recv, "big", signed=False)
			print("got can recv id: %d (0x%x)" % (id, id))
			return id
		else:
			print("failed to read")
			return None
		
	def check_sample_freq(self):
		print("checking sample frequency...")
		recv = self.read_frame(self.addr, 0x07, 1)
		if recv:
			level = int.from_bytes(recv, "big", signed=False)
			if level < 0 or level > 4:
				print("read sample frequency failed, result invalid")
				return None
			freq = ["single", "5Hz", "10Hz", "20Hz", "30Hz"]
			print("got sample frequency: %s (level %d)" % (freq[level], level))
			return level
		else:
			print("failed to read")
			return None


TTY_PATH = "/dev/ttyUSB0"
DEV_ID = 1
CAN_BAUD = 500
CAN_SEND_ID = 646
CAN_RECV_ID = 774

if __name__ == "__main__":
	
	if len(sys.argv) < 6:
		print("usage: %s [tty_path] [laser id] [can baud] [can send id] [can recv id]" % (sys.argv[0]))
		print("using default parameters:")
	else:
		TTY_PATH = sys.argv[1]
		DEV_ID = int(sys.argv[2])
		CAN_BAUD = int(sys.argv[3])
		CAN_SEND_ID = int(sys.argv[4])
		CAN_RECV_ID = int(sys.argv[5])

	print("tty path: %s, laser id: %d, can baud: %d kbps, can send id: %d (0x%x), can recv id: %d (0x%x)" % \
	    		(TTY_PATH, DEV_ID, CAN_BAUD, CAN_SEND_ID, CAN_SEND_ID, CAN_RECV_ID, CAN_RECV_ID))

	tty = laser(TTY_PATH, DEV_ID)

	# check
	tty.check_sample_freq()
	tty.check_can_baud()
	tty.check_can_send_id()
	tty.check_can_recv_id()

	# config
	tty.config_sample_freq(4)
	tty.config_can_baud(CAN_BAUD)
	tty.config_can_send_id(CAN_SEND_ID)
	tty.config_can_recv_id(CAN_RECV_ID)

	# validate
	tty.check_sample_freq()
	tty.check_can_baud()
	tty.check_can_send_id()
	tty.check_can_recv_id()
	
	# save to rom
	tty.save_rom()
	
	print("everything done")




